#include "coordinate_transformer.h"
#include <stdio.h>
#include <stdint.h>
// ==================== 数学常量定义 ====================
double DEG_TO_RAD = M_PI / 180.0;   // 度转弧度
double RAD_TO_DEG = 180.0 / M_PI;   // 弧度转度
double EARTH_RADIUS = 6371000.0;    // 地球平均半径(米)
double SCALING_FACTOR = 1e-7;       // 缩放因子
double DEG_TO_METER = 111320.0;     // 1度转米, DEG_TO_RAD * RADIUS_OF_EARTH
double METER_TO_DEG = 1 / 111320.0; // 1米转度, RAD_TO_DEG / RADIUS_OF_EARTH

typedef struct
{
       float x; // x轴分量
       float y; // y轴分量
} Vector2f;

// 地理位置结构体
typedef struct
{
       int32_t latitude;  // 纬度(10^7度)
       int32_t longitude; // 经度(10^7度)
       int32_t altitude;  // 高度(厘米)
} Location;

/**
 * @brief   计算指定纬度处的经度缩放因子
 * @param   loc 参考位置
 * @return  缩放因子(0.01~1.0)
 * @note    基于纬度余弦值计算
 */
static double longitudeScaleFactor(const Location loc);

/**
 * @brief   计算两个位置之间的距离
 * @param   loc1 第一个位置
 * @param   loc2 第二个位置
 * @return  地面距离(米)
 * @note    使用Haversine公式：
 *          a = sin²(Δφ/2) + cosφ1·cosφ2·sin²(Δλ/2)
 *          d = 2R·asin(√a)
 */
static double calculateDistance(const Location loc1, const Location loc2);

/**
 * @brief   计算从起点到终点的方位角
 * @param   loc1 起始位置
 * @param   loc2 目标位置
 * @return  方位角，单位为厘度(0-36000)
 * @note    0表示正北，9000表示正东，即从正北开始顺时针的角度
 */
static int32_t calculateBearing(const Location start, const Location end);

/**
 * @brief   偏移当前位置
 * @param   loc 原始位置(将被修改)
 * @param   ofs_north 北向偏移量(米)
 * @param   ofs_east 东向偏移量(米)
 * @note    原地修改位置数据
 */
static void offsetLocation(Location loc, const double ofs_north, const double ofs_east);

/**
 * @brief   生成偏移后的新坐标
 * @param   loc 原始位置
 * @param   ofs_north 北向偏移(米)
 * @param   ofs_east 东向偏移(米)
 * @param   ret 返回的新坐标
 * @note    不修改原始位置
 */
static void addLocation(const Location loc, const double ofs_north, const double ofs_east, Location ret);

/**
 * @brief   计算两个位置之间的北东平面坐标差
 * @param   loc1 第一个位置指针
 * @param   loc2 第二个位置指针
 * @return  loc2-loc1 二维向量差值(北向差值,东向差值)
 * @note    返回单位为米的平面差值
 */
static Vector2f diffLocation(const Location loc1, const Location loc2);

double math_constrain_double(double value, double min, double max)
{
       double ret = value;
       if (value < min)
              ret = min;
       if (value > max)
              ret = max;
       return ret;
}

double longitudeScaleFactor(const Location loc)
{
       // 厘度->度
       double latitude_deg = loc.latitude * SCALING_FACTOR;
       double scale = cos(latitude_deg * SCALING_FACTOR * DEG_TO_RAD);
       return math_constrain_double(scale, 0.01, 1.0);
}

double calculateDistance(const Location loc1, const Location loc2)
{
       // 计算经度和纬度差(厘度->度->弧度)
       double lat1_rad = loc1.latitude * SCALING_FACTOR * DEG_TO_RAD;
       double lon1_rad = loc1.longitude * SCALING_FACTOR * DEG_TO_RAD;
       double lat2_rad = loc2.latitude * SCALING_FACTOR * DEG_TO_RAD;
       double lon2_rad = loc2.longitude * SCALING_FACTOR * DEG_TO_RAD;

       double lat = lat2_rad - lat1_rad;
       double lon = lon2_rad - lon1_rad;

       double a = sin(lat / 2.0) * sin(lat / 2.0) +
                  cos(lat1_rad) * cos(lat2_rad) * sin(lon / 2.0) * sin(lon / 2.0);
       double c = 2 * atan2(sqrt(a), sqrt(1 - a));

       return c * EARTH_RADIUS;
}

int32_t calculateBearing(const Location start, const Location end)
{
       // 计算经度和纬度差(厘度->度->弧度)
       double lat1_rad = start.latitude * SCALING_FACTOR * DEG_TO_RAD;
       double lon1_rad = start.longitude * SCALING_FACTOR * DEG_TO_RAD;
       double lat2_rad = end.latitude * SCALING_FACTOR * DEG_TO_RAD;
       double lon2_rad = end.longitude * SCALING_FACTOR * DEG_TO_RAD;

       double lon = lon2_rad - lon1_rad;
       // 标准球面三角公式计算方位角
       double y = sin(lon) * cos(lat2_rad);
       double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(lon);

       double bearing_rad = atan2(y, x);
       double bearing_deg = bearing_rad * RAD_TO_DEG;
       int32_t bearing_centidegrees = (int32_t)(round(bearing_deg * 100));

       bearing_centidegrees %= 36000;
       if (bearing_centidegrees < 0)
       {
              bearing_centidegrees += 36000;
       }

       return bearing_centidegrees;
}

void offsetLocation(Location location, const double ofs_north, const double ofs_east)
{
       // 计算纬度偏移(米->度->厘度)
       double lat_offset_centi_deg = ofs_north * METER_TO_DEG / SCALING_FACTOR;
       // 计算经度偏移(米->度->厘度)
       double lon_offset_centi_deg = ofs_east * METER_TO_DEG / longitudeScaleFactor(location) / SCALING_FACTOR;
       // 更新位置
       location.latitude += (int32_t)(lat_offset_centi_deg);
       location.longitude += (int32_t)(lon_offset_centi_deg);
}

void addLocation(const Location loc, const double ofs_north,
                 const double ofs_east, Location ret)
{
       ret = loc;
       offsetLocation(ret, ofs_north, ofs_east);
}

Vector2f diffLocation(const Location loc1, const Location loc2)
{
       // 计算纬度差(厘度->度->米)
       double lat_diff_deg = (loc2.latitude - loc1.latitude) * SCALING_FACTOR;
       double north_diff = lat_diff_deg * DEG_TO_METER;
       // 考虑纬度缩放
       Location avg;
       avg.altitude = 0;
       avg.latitude = 0;
       avg.longitude = 0;
       avg.latitude = (loc1.latitude + loc2.latitude) / 2 * SCALING_FACTOR;
       // 计算经度差(厘度->度->米)
       double lon_diff_deg = (loc2.longitude - loc1.longitude) * SCALING_FACTOR;
       double east_diff = lon_diff_deg * DEG_TO_METER * longitudeScaleFactor(avg);
       Vector2f tmp;
       tmp.x = north_diff;
       tmp.y = east_diff;
       return tmp;
}

void test_all_points()
{
       printf("=== 开始坐标转换精度测试 ===\n\n");

       // ===== 设置基准点
       Base_LLA base;
       base.base_lon = 116.3914;
       base.base_lat = 39.9056;
       base.base_alt = 50.0;

       Vector3d base_point = {base.base_lon, base.base_lat, base.base_alt};

       // ===== 测试点定义
       Vector3d north_point = {116.3914, 39.9146, 50.0}; // 正北1km
       Vector3d east_point = {116.4024, 39.9056, 50.0};  // 正东1km
       Vector3d up_point = {116.3914, 39.9056, 150.0};   // 正上100m

       // ===== 转换基准点为Location格式（用于Haversine）
       Location base_loc;
       base_loc.longitude = (int32_t)(base.base_lon * 1e7);
       base_loc.latitude = (int32_t)(base.base_lat * 1e7);
       base_loc.altitude = (int32_t)(base.base_alt * 100);

       // ===== 测试1: 正北点
       printf("1. 测试正北点:\n");
       printf("   基准点: [%.6f, %.6f, %.2f]\n", base_point.x, base_point.y, base_point.z);
       printf("   正北点: [%.6f, %.6f, %.2f]\n", north_point.x, north_point.y, north_point.z);

       // NED方法
       float dist_ned = cal_3d_distance(&base, &base_point, &north_point);
       float horizontal_dist_ned = cal_horizontal_distance(&base, &base_point, &north_point);
       float bearing_ned = cal_bearing(&base, &base_point, &north_point);
       float elevation_ned = cal_elevation(&base, &base_point, &north_point);

       printf("   NED方法 - 三维距离: %.3f m, 水平距离: %.3f m, 方位角: %.2f°, 俯仰角: %.2f°\n",
              dist_ned, horizontal_dist_ned, bearing_ned, elevation_ned);

       // Haversine方法
       Location north_loc;
       north_loc.longitude = (int32_t)(north_point.x * 1e7);
       north_loc.latitude = (int32_t)(north_point.y * 1e7);
       north_loc.altitude = (int32_t)(north_point.z * 100);

       float horizontal_dist_hav = calculateDistance(base_loc, north_loc);
       int32_t bearing_centideg = calculateBearing(base_loc, north_loc);
       float bearing_hav = bearing_centideg / 100.0f;

       printf("   Haversine - 水平距离: %.3f m, 方位角: %.2f°\n",
              horizontal_dist_hav, bearing_hav);

       // 理论计算
       // double lat_diff = 39.9146 - 39.9056;
       // double theoretical_dist = lat_diff * 111.32 * 1000;
       // printf("   理论计算 - 水平距离: ≈%.2f m, 方位角: 0.0°\n", theoretical_dist);
       // printf("   NED与理论偏差: %.2f m, Haversine与理论偏差: %.2f m\n\n",
       //        fabs(horizontal_dist_ned - theoretical_dist),
       //        fabs(horizontal_dist_hav - theoretical_dist));

       // ===== 测试2: 正东点
       printf("2. 测试正东点:\n");
       printf("   正东点: [%.6f, %.6f, %.2f]\n", east_point.x, east_point.y, east_point.z);

       // NED方法
       dist_ned = cal_3d_distance(&base, &base_point, &east_point);
       horizontal_dist_ned = cal_horizontal_distance(&base, &base_point, &east_point);
       bearing_ned = cal_bearing(&base, &base_point, &east_point);
       elevation_ned = cal_elevation(&base, &base_point, &east_point);

       printf("   NED方法 - 三维距离: %.3f m, 水平距离: %.3f m, 方位角: %.2f°, 俯仰角: %.2f°\n",
              dist_ned, horizontal_dist_ned, bearing_ned, elevation_ned);

       // Haversine方法
       Location east_loc;
       east_loc.longitude = (int32_t)(east_point.x * 1e7);
       east_loc.latitude = (int32_t)(east_point.y * 1e7);
       east_loc.altitude = (int32_t)(east_point.z * 100);

       horizontal_dist_hav = calculateDistance(base_loc, east_loc);
       bearing_centideg = calculateBearing(base_loc, east_loc);
       bearing_hav = bearing_centideg / 100.0f;

       printf("   Haversine - 水平距离: %.3f m, 方位角: %.2f°\n",
              horizontal_dist_hav, bearing_hav);

       // // 理论计算（正东方向需要考虑纬度影响）
       // double lon_diff = 116.4024 - 116.3914;
       // double avg_lat_rad = (39.9056 + 39.9056) / 2.0 * DEG_TO_RAD;
       // theoretical_dist = lon_diff * 111.32 * 1000 * cos(avg_lat_rad);
       // printf("   理论计算 - 水平距离: ≈%.2f m, 方位角: 90.0°\n", theoretical_dist);
       // printf("   NED与理论偏差: %.2f m, Haversine与理论偏差: %.2f m\n\n",
       //        fabs(horizontal_dist_ned - theoretical_dist),
       //        fabs(horizontal_dist_hav - theoretical_dist));

       // ===== 测试3: 正上点
       printf("3. 测试正上点:\n");
       printf("   正上点: [%.6f, %.6f, %.2f]\n", up_point.x, up_point.y, up_point.z);

       // NED方法（这是NED的优势，可以计算三维信息）
       dist_ned = cal_3d_distance(&base, &base_point, &up_point);
       horizontal_dist_ned = cal_horizontal_distance(&base, &base_point, &up_point);
       bearing_ned = cal_bearing(&base, &base_point, &up_point);
       elevation_ned = cal_elevation(&base, &base_point, &up_point);

       printf("   NED方法 - 三维距离: %.3f m, 水平距离: %.3f m, 方位角: %.2f°, 俯仰角: %.2f°\n",
              dist_ned, horizontal_dist_ned, bearing_ned, elevation_ned);

       // Haversine方法（对于正上点，水平距离应该为0）
       Location up_loc;
       up_loc.longitude = (int32_t)(up_point.x * 1e7);
       up_loc.latitude = (int32_t)(up_point.y * 1e7);
       up_loc.altitude = (int32_t)(up_point.z * 100);

       horizontal_dist_hav = calculateDistance(base_loc, up_loc);
       bearing_centideg = calculateBearing(base_loc, up_loc);
       bearing_hav = bearing_centideg / 100.0f;

       printf("   Haversine - 水平距离: %.3f m, 方位角: %.2f°\n",
              horizontal_dist_hav, bearing_hav);
       // printf("   理论计算 - 水平距离: 0.0 m, 垂直距离: 100.0 m\n");
       printf("   NED垂直距离: %.2f m\n", dist_ned);
}

int main()
{
       test_all_points();

       // // 设置基准点
       // Base_LLA base;
       // base.base_lon = 116.3914;
       // base.base_lat = 39.9056;
       // base.base_alt = 50.0;

       // // 测试点1 - 正北
       // Vector3d north_point = {116.3914, 39.9146, 50.0};
       // Vector3d north_ned;
       // lla_to_ned(&base, &north_point, &north_ned);
       // printf("北向点 NED: [%.2f, %.2f, %.2f]\n", north_ned.x, north_ned.y, north_ned.z);

       // // 测试点2 - 正东
       // Vector3d east_point = {116.4024, 39.9056, 50.0};
       // Vector3d east_ned;
       // lla_to_ned(&base, &east_point, &east_ned);
       // printf("东向点 NED: [%.2f, %.2f, %.2f]\n", east_ned.x, east_ned.y, east_ned.z);

       // // 测试点3 - 正上
       // Vector3d up_point = {116.3914, 39.9056, 150.0};
       // Vector3d up_ned;
       // lla_to_ned(&base, &up_point, &up_ned);
       // printf("上方点 NED: [%.2f, %.2f, %.2f]\n", up_ned.x, up_ned.y, up_ned.z);

       // printf("=== 反向转换验证 ===\n");

       // // 测试点1 - 北向点反向
       // Vector3d ned_north = {999.30399890, -0.00000000, 0.07848517};
       // Vector3d lla_north_back;
       // ned_to_lla(&base, &ned_north, &lla_north_back);
       // printf("北向点反向LLA: [%.8f, %.8f, %.2f]\n",
       //        lla_north_back.x, lla_north_back.y, lla_north_back.z);

       // // 测试点2 - 东向点反向
       // Vector3d ned_east = {0.05792595, 940.63200805, 0.06926490};
       // Vector3d lla_east_back;
       // ned_to_lla(&base, &ned_east, &lla_east_back);
       // printf("东向点反向LLA: [%.8f, %.8f, %.2f]\n",
       //        lla_east_back.x, lla_east_back.y, lla_east_back.z);

       // // 测试点3 - 上方点反向
       // Vector3d ned_up = {0.00000000, 0.00000000, -100.00000000};
       // Vector3d lla_up_back;
       // ned_to_lla(&base, &ned_up, &lla_up_back);
       // printf("上方点反向LLA: [%.8f, %.8f, %.2f]\n",
       //        lla_up_back.x, lla_up_back.y, lla_up_back.z);

       // Aircraft aircraft;
       // aircraft.altitude = 500.0;
       // aircraft.latitude = 39.9056;
       // aircraft.longitude = 116.3914;
       // aircraft.id = 1;
       // aircraft.pitch = 5.0;
       // aircraft.roll = 30.0;
       // aircraft.yaw = 20.0;
       // // 验证两个矩阵是否互逆
       // Matrix3d R_forward, R_inverse;
       // set_rotation_ned_to_body(&aircraft, &R_forward);
       // set_rotation_body_to_ned(&aircraft, &R_inverse);

       // // 计算乘积，应该是单位矩阵
       // Matrix3d identity;
       // matrix_multiply_3x3(&R_forward, &R_inverse, &identity);

       // // 检查identity是否接近单位矩阵
       // for (int i = 0; i < 3; i++)
       // {
       //     for (int j = 0; j < 3; j++)
       //     {
       //         if (identity.data[i][j] > 1e-2)
       //             printf("%.2f  ", identity.data[i][j]);
       //         else if ((fabs(identity.data[i][j]) < 1e-10))
       //             printf("---  ");
       //         else
       //             printf("///  ");
       //     }
       //     printf("\n");
       // }

       // // 测试一个已知向量在两个坐标系下的变换
       // Vector3d test_ned = {1000.0, 0.0, 0.0}; // 北方向1000米

       // Vector3d test_body, test_back;
       // Matrix3d R_ned_to_body, R_body_to_ned;

       // set_rotation_ned_to_body(&aircraft, &R_ned_to_body);
       // set_rotation_body_to_ned(&aircraft, &R_body_to_ned);

       // // NED -> Body -> NED，应该回到原值
       // matrix3d_mul_vector3d(&R_ned_to_body, &test_ned, &test_body);
       // matrix3d_mul_vector3d(&R_body_to_ned, &test_body, &test_back);

       // printf("Original: %.2f, %.2f, %.2f\n", test_ned.x, test_ned.y, test_ned.z);
       // printf("Back: %.2f, %.2f, %.2f\n", test_back.x, test_back.y, test_back.z);
       // // 这两个应该非常接近

       return 0;
}